﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using NIP.ControllerInterface;
using NIP.Speech;
using System.Threading;
using System.Diagnostics;
using NIP.DynamicControl;
using NIP.DynamicControl.Environ;
using NIP.Speech.Dialog;
using OpenCvSharp;
using NIP.VisionSharp;
using NIP.DynamicControl.Constants;

//using NIP.VisionWrapper;
//using RSD.Robot;
namespace NIP.ControlLogic
{
    public class BasicMoveControlLogic: IControlLogic
    {
        #region Fields
        private Manager speechObject;
        public RobotManager robotObject;
        public  VisionSharp.Potraitor visionObject;

        //private const RobotControlEventArgs BrushPaintDipPosition =
        //    new RobotControlEventArgs(460.0, -170.0, 190.0, -90, -180.0, 150);
        //private const RobotControlEventArgs BrushPaintDipPositionPrep =
        //    new RobotControlEventArgs(460.0, -170.0, 290, -90, -180.0, 150);

        private readonly RobotControlEventArgs TakePhotoPosition = 
            new RobotControlEventArgs(ControlCommands.TAKE_PHOTO, 700, 100, 800, 10, -180, 150);
            

        #endregion

        #region Events

        /// <summary>
        /// Occurs when the state of the current Logic changed
        /// </summary>
        public event EventHandler StateChanged;

        /// <summary>
        /// Occurs when a unspecified action have started.
        /// </summary>
        public event EventHandler ActionStarted;

        /// <summary>
        /// Occurs when a unspecified action have completed.
        /// </summary>
        public event EventHandler ActionCompleted;

        public event EventHandler<RobotStatusUpdateEventArgs> RobotStatusUpdate;
 

        #endregion

        #region Constructors Destructors
        public BasicMoveControlLogic()
        {
            InitializeParameters();
            speechObject = new Speech.Manager();
            robotObject = new DynamicControl.RobotManager();
            visionObject = new VisionSharp.Potraitor();
            Console.WriteLine("Control Logic created");
            #region Old Code with RSD.Robot.RobotController
            //var controller = new RobotController();
            //controller.InitializationCompleted += new EventHandler<InitializationCompletedEventArgs>(controllerObject_InitializationCompleted);
            //try
            //{
            //    controller.Initialization(RSD.Robot.InitializationMode.Online, SystemType.Default);
            //    Thread.Sleep(2000);
            //    controller.SetControl(Axis.All, true);
            //}
            //catch (Exception e)
            //{
            //    Debug.WriteLine(e.Message);
            //}
            #endregion
            speechObject.dialogManager.SpeechActionRequested += new EventHandler<Speech.EventArgs.SpeechActionRequestedEventArgs>(SpeechActionRequested_EventHandler);
            this.RobotStatusUpdate += new EventHandler<RobotStatusUpdateEventArgs>(speechObject.dialogManager.RobotStatusUpdate_EventHandler);
            this.RobotStatusUpdate += new EventHandler<RobotStatusUpdateEventArgs>(visionObject.RobotMovementCompleted_Handler);
            speechObject.dialogManager.SpeechActionRequested += new EventHandler<Speech.EventArgs.SpeechActionRequestedEventArgs>(visionObject.SpeechActionRequest_Handler);
            visionObject.ContourReadyDrawEvent += new EventHandler<DrawEventArgs>(OnContourReadyDrawEvent);
            robotObject.PathPlanningObject.PathPlanningRobotStatusUpdated += new EventHandler<RobotStatusUpdateEventArgs>(visionObject.CommandCompletedRobotStatusUpdateHandler);
            robotObject.PathPlanningObject.PathPlanningRobotStatusUpdated += new EventHandler<RobotStatusUpdateEventArgs>(speechObject.dialogManager.RobotStatusUpdate_EventHandler);


           OnEnablePIDActionRequest(new RobotControlEventArgs());


            #region Vision
            //VWrapper.setupKinect();

            //var rgbWin = new OpenCvSharp.CvWindow("RGB",OpenCvSharp.WindowMode.AutoSize);
            //var depthWin = new OpenCvSharp.CvWindow("DEPTH", OpenCvSharp.WindowMode.AutoSize);
            //var threshWin = new OpenCvSharp.CvWindow("THRESH", OpenCvSharp.WindowMode.AutoSize);
            //var armCamWin = new OpenCvSharp.CvWindow("ARMCAM", OpenCvSharp.WindowMode.AutoSize);

            //IplImage RGBImage = null;
            //IplImage DepthImage = null;
            //IplImage DepthImageGrey = null;
            //IplImage Threshed = new OpenCvSharp.IplImage(new CvSize(640, 480), (BitDepth)8, 1);
            //IplImage robotCam = VWrapper.getArmCam();

            

            #endregion

        }

        private void InitializeParameters()
        {
        }
        ~BasicMoveControlLogic()
        {
            Console.WriteLine("Control Logic destroyed");
        }
        #endregion

        #region NIP.Speech Handlers
        public void RecoObject_CommandRecognized(object sender, Speech.EventArgs.CommandRecognizedEventArgs args)
        {
            throw new NotImplementedException();
            //switch (args.Action)
            //{
            //    case "MOVE":
            //        if (args.Direction != "NONE")
            //        {
            //            OnMoveActionRequest(args.Direction);
            //        }
            //        break;
            //    case "GET":
            //        if (args.Object != "NONE")
            //        {
            //            OnGetActionRequest(args.Object, args.Target);
            //        }
            //        break;
            //    case "HOME":
            //        OnHomeActionRequest();
            //        break;
            //    case "STOP":
            //        OnStopActionRequest();
            //        break;
            //    default:
            //        break;

            //}

        }
       
        private void OnStopActionRequest()
        {
            throw new NotImplementedException();
        }
        private void OnMoveActionRequest(string dir)
        {
            throw new NotImplementedException();
            //PathPoint newPos = new PathPoint();// = controllerObject.GetcurrentPosition(); //
            //int whichIndex = 0;
            //const int moveDist = 20000;//10cm
            //int sign = 1;

            //switch (dir)
            //{
            //    case "UP":
            //        whichIndex = 2; //Z
            //        sign = 1;
            //        break;
            //    case "DOWN":
            //        whichIndex = 2; //Z
            //        sign = -1;
            //        break;
            //    case "LEFT":
            //        whichIndex = 1; //Y
            //        sign = 1;
            //        break;
            //    case "RIGHT":
            //        whichIndex = 1; //Y
            //        sign = -1;
            //        break;
            //    case "BWD":
            //        whichIndex = 0; //X
            //        sign = -1;
            //        break;
            //    case "FWD":
            //        whichIndex = 0; //X
            //        sign = 1;
            //        break;
            //    default:
            //        break;
            //}

            
            //newPos.XYZ[whichIndex] += moveDist * sign;
            //robotObject.controllerObject.Speed((byte)Axis.All, (long)20);
            //robotObject.controllerObject.MoveToPosition(newPos, PointType.RelativeXYZCoordinate);
            //PathPoint currentPos = new PathPoint();
            //currentPos = robotObject.PIDObject.CurrentPos.get();
            //Debug.WriteLine("Current ang:" + currentPos.Joint[0] + " " + currentPos.Joint[1] + " " + currentPos.Joint[2] + " " + currentPos.Joint[3] + " " + currentPos.Joint[4]);
            //Debug.WriteLine("Current pos:" + currentPos.XYZ[0] + " " + currentPos.XYZ[1] + " " + currentPos.XYZ[2]);
        }

        private void OnGetActionRequest(string p, string p_2)
        {
            throw new NotImplementedException();
        }
        

        #endregion

        #region NIP.Robot handlers

        public void Robot_CommandRecognized(object sender, RobotControlEventArgs args)
        {
            Console.Write("COMMAND:");
            switch (args.command)
            {
                case ControlCommands.MOVE_TO:
                    {
                        //EnqueueAction(new Action<RobotControlEventArgs>(OnMoveToActionRequest),args);
                        Console.WriteLine("MOVE TO " + args.X + "," + args.Y + "," + args.Z);
                        RobotStatusUpdate(robotObject, new RobotStatusUpdateEventArgs("MOVE TO " + args.X + "," + args.Y + "," + args.Z));
                        OnMoveToActionRequest(args);
                    }
                    break;
                case ControlCommands.LOOK_AT :
                    {
                        Console.WriteLine("LOOK AT " + args.X + "," + args.Y + "," + args.Z);
                        OnLookAtActionRequest(args);
                    }
                    break;
                case ControlCommands.POINT_TO:
                    {
                        Console.WriteLine("POINT TO " + args.X + "," + args.Y + "," + args.Z);
                        OnPointToActionRequest(args);
                    }
                    break;
                case ControlCommands.TEST_PID:
                    {
                        Console.WriteLine("TEST PID");
                        OnTestPIDActionRequest(args);
                    }
                    break;
                case ControlCommands.ENABLE_PID :
                    {
                        Console.WriteLine("ENABLE PID");
                        RobotStatusUpdate(robotObject, new RobotStatusUpdateEventArgs("ENABLE PID"));
                        OnEnablePIDActionRequest(args);
                    }
                    break;
                case ControlCommands.SET_PID:
                    {
                        Console.WriteLine("SET PID");
                        RobotStatusUpdate(robotObject, new RobotStatusUpdateEventArgs("SET PID"));
                        OnSetPIDActionRequest(args);
                    }
                    break;
                case ControlCommands.HOME:
                    {
                        Console.WriteLine("HOME");
                        RobotStatusUpdate(robotObject, new RobotStatusUpdateEventArgs("HOME"));
                        OnHomeActionRequest();
                    }
                    break;
                case ControlCommands.GRIP:
                    {
                        Console.WriteLine("TOGGLE GRIPPER");
                        RobotStatusUpdate(robotObject, new RobotStatusUpdateEventArgs("TOGGLE GRIPPER"));
                        OnGripActionRequest();
                    }
                    break;
                case ControlCommands.DRAW:
                    {
                        Console.WriteLine("DRAW");
                        //RobotStatusUpdate(robotObject, new RobotStatusUpdateEventArgs("DRAW"));
                        //OnContourReadyDrawEvent(args);


                        //! PRESUME THIS IS A SPEECH EVENT - BUT COME FROM UI
                        OnDrawEventCommandReceived();

                    }
                    break;

                        
                default:
                    break;

            }

        }
        public void SpeechActionRequested_EventHandler(object sender, Speech.EventArgs.SpeechActionRequestedEventArgs sArgs)
        {
            Console.Write("COMMAND: ");
            RobotControlEventArgs args = new RobotControlEventArgs();
            args.command = sArgs.command;
            switch (args.command)
            {                
                case ControlCommands.HOME:
                    {
                        Console.WriteLine("HOME");
                        OnHomeActionRequest();
                    }
                    break;
                case ControlCommands.HELLO:
                    {
                        OnHelloRequest();
                    }
                    break;
                case ControlCommands.GRIP:
                    {
                        Console.WriteLine("TOGGLE GRIPPER");
                        OnGripActionRequest();
                    }
                    break;
               case ControlCommands.ABORT:
                    {
                        Console.WriteLine("ABORT");
                        throw new NotImplementedException();
                    }
                    break;
               case ControlCommands.DRAW:
                    {
                        Console.WriteLine("DRAW");
                        OnDrawEventCommandReceived();
                    }
                    break;
               case ControlCommands.STOP:
                    {
                        Console.WriteLine("STOPPING");
                        throw new NotImplementedException();
                    }
                    break;

                default:
                    break;

            }
        }

        private void OnHelloRequest()
        {
            //! First position
           //OnMoveToActionRequest(HelloFirstPosition);
           //Thread.Sleep(1000);
           //OnMoveToActionRequest(HelloSecondPosition);
        }

        public void OnContourReadyDrawEvent(object sender, DrawEventArgs args)
        {
            //Console.Beep();
            int contours = args.PointList.Count;
            double[] lastXYZ = new double[3];
            int contour;
            double xmax = 0.0;
            double xmin = 0.0;
            double ymax = 0.0;
            double ymin = 0.0;
            for (int i = 0; i < contours; i++)
            {
                int c = args.PointList[i].Count;
                for (int j = 0; j < c; j++)
                {
                    if (args.PointList[i][j][0] > xmax)
                    {
                        xmax = args.PointList[i][j][0];
                    }
                    if (args.PointList[i][j][0] < xmin)
                    {
                        xmin = args.PointList[i][j][0];
                    }
                    if (args.PointList[i][j][1] > ymax)
                    {
                        ymax = args.PointList[i][j][1];
                    }
                    if (args.PointList[i][j][1] < ymin)
                    {
                        ymin = args.PointList[i][j][1];
                    }

                }
            }
            Debug.WriteLine("Drawing dimensions: X " + xmax + "," + xmin + " Y " + ymax + "," + ymin);
            // example output: Drawing dimensions: X 126.582901554404,-132.116580310881 Y 63.699481865285,-193.616580310881
            RobotControlEventArgs controlCommand = new RobotControlEventArgs();
            controlCommand.X = gridconst.imgcenterX;
            controlCommand.Y = gridconst.imgcenterY;
            controlCommand.Z = gridconst.imgcenterZ + gridconst.DrawContourZOffset;
            controlCommand.gPitch = -90.0;
            controlCommand.gRoll = 0.0;
            controlCommand.gOffset = 0.0;
            controlCommand.command = DynamicControl.ControlCommands.MOVE_TO;
            OnMoveToActionRequest(controlCommand);
            
            //move to first contour
            //robotObject.PathPlanningObject.BuildContourTransitionSection(
            for (contour = 0; contour < contours; contour++)
            {
                //Debug.WriteLine("insert moveto next contour");

                controlCommand.X = gridconst.imgcenterX + args.PointList[contour][0][1];
                controlCommand.Y = gridconst.imgcenterY + args.PointList[contour][0][0];
                controlCommand.Z = gridconst.imgcenterZ + gridconst.DrawContourZOffset;
                controlCommand.gPitch = -90.0;
                controlCommand.gRoll = 0.0;
                controlCommand.gOffset = 0.0;
                controlCommand.command = DynamicControl.ControlCommands.MOVE_TO;
                OnMoveToActionRequest(controlCommand);

                controlCommand.X = gridconst.imgcenterX + args.PointList[contour][0][1];
                controlCommand.Y = gridconst.imgcenterY + args.PointList[contour][0][0];
                controlCommand.Z = gridconst.imgcenterZ;
                controlCommand.gPitch = -90.0;
                controlCommand.gRoll = 0.0;
                controlCommand.gOffset = 0.0;
                controlCommand.command = DynamicControl.ControlCommands.MOVE_TO;
                OnMoveToActionRequest(controlCommand);

                robotObject.PathPlanningObject.BuildContourPath(args.PointList[contour]);
                //move to next contour
                //robotObject.PathPlanningObject.BuildContourTransitionSection(args.PointList[contour].Last(), args.PointList[contour + 1][0]);
                lastXYZ = args.PointList[contour].Last();

                controlCommand.X = gridconst.imgcenterX + lastXYZ[1];
                controlCommand.Y = gridconst.imgcenterY + lastXYZ[0];
                controlCommand.Z = gridconst.imgcenterZ + gridconst.DrawContourZOffset;
                controlCommand.gPitch = -90.0;
                controlCommand.gRoll = 0.0;
                controlCommand.gOffset = 0.0;
                controlCommand.command = DynamicControl.ControlCommands.MOVE_TO;
                OnMoveToActionRequest(controlCommand);

                

            }
            var status = new RobotStatusUpdateEventArgs("Your portrait should be ready soon. The charge is 2000 dollars. Thank you.");
            if(RobotStatusUpdate!= null)
            {
                RobotStatusUpdate(new object(),status);
            }
            //controlCommand.X = gridconst.imgcenterX;
            //controlCommand.Y = gridconst.imgcenterY;
            //controlCommand.Z = gridconst.imgcenterZ + (2.0*gridconst.DrawContourZOffset);
            //controlCommand.gPitch = -90.0;
            //controlCommand.gRoll = 0.0;
            //controlCommand.gOffset = 0.0;
            //controlCommand.command = DynamicControl.ControlCommands.MOVE_TO;
            //OnMoveToActionRequest(controlCommand);
        }
        private void OnDrawEventCommandReceived()
        {
            //throw new NotImplementedException();
            
            // Move to PhotoTake position

            //! Wait till bot stop
            //while (robotObject.PIDObject.InMotion)
            //{
            //    Thread.Sleep(100);
            //}

            //! Add to queue
            OnMoveToActionRequest(TakePhotoPosition);
            Debug.WriteLine("Draw command received");
            //Thread.Sleep(500);

            //! Wait till bot stop
            //while (robotObject.PIDObject.InMotion)
            //{
            //    Thread.Sleep(100);
            //}

            // When position reached fire event
            //if (this.RobotStatusUpdate != null)
            //{
            //    var args = new RobotStatusUpdateEventArgs("in position for taking photo");
            //    RobotStatusUpdate(new object(), args);
            //}
        }


        private void OnGripActionRequest()
        {

            robotObject.ToggleGripper(); 
            //if (robotObject.GripClosed)
            //{
            //    //robotObject.PIDObject.OpenGripper = true;
            //    robotObject.controllerObject.OpenGripper();
            //    //Console.WriteLine("Gripper Closed ? " + robotObject.GripClosed);
            //    //! Disabled due to use of GetJaw()
            //    // robotObject.GripClosed = false;
            //}
            //else if (!robotObject.GripClosed)
            //{
            //    //robotObject.PIDObject.CloseGripper = true;
            //    robotObject.controllerObject.CloseGripper();
            //    //Console.WriteLine("Gripper Closed ? " + robotObject.GripClosed);
            //    //! Disabled due to use of GetJaw()
            //    //robotObject.GripClosed = true;
            //}
            //else
            //    return;
        


        }

        private void OnEnablePIDActionRequest(RobotControlEventArgs args)
        {
            if (robotObject.PIDthread.IsAlive)
            {
                if (robotObject.PIDObject.IsActive())
                {
                    robotObject.PIDObject.RequestStop();
                    while (robotObject.PIDObject.IsActive()) ;
                    Console.WriteLine("JointPID thread deactivated");
                }
                else
                {
                    robotObject.PIDObject.RequestStart();
                    while (!robotObject.PIDObject.IsActive()) ;
                    Console.WriteLine("JointPID thread activated");
                }                
            }
            else
            {
                robotObject.PIDthread.Start();
                while (!robotObject.PIDthread.IsAlive) ;
                Console.WriteLine("JointPID thread activated");
            }
        }

        private void OnTestPIDActionRequest(RobotControlEventArgs args)
        {

            Debug.WriteLine("testpid write torque array int[]");
            //int[] jointPWM = new int[USBC.TotalAxes] { 1275, 1275, 1275, 1275, 1275, 0, 0, 0 };
            //robotObject.controllerObject.MoveTorque(jointPWM, USBC.TotalAxes); //this doesn't work; jerks and shuts donw

        }

        private void OnMoveToActionRequest(RobotControlEventArgs args)
        {
            DateTime startTime;
            DateTime inter1, inter2, inter3;
            TimeSpan time1, time2, time3;
            PathPoint tempGoal = new PathPoint();// { XYZd = new double[USBC.TotalAxes] { args.X, args.Y, args.Z, args.gPitch, args.gRoll, args.gOffset, 0, 0 } };
            tempGoal.pos[0] = args.X;
            tempGoal.pos[1] = args.Y;
            tempGoal.pos[2] = args.Z;
            tempGoal.WristPitch = args.gPitch;
            tempGoal.setRoll(args.gRoll);

            //Debug.WriteLine("MoveToCoords:" + args.X + "," + args.Y + "," + args.Z + "; Roll:" + args.gRoll + " Pitch:" + args.gPitch);
            //PathPoint tempGoal = new PathPoint(tempG);

            
            //Position relPos = new Position();



            
            if (DynamicControl.InverseKinematics.Inv_K.solveMoveToJointAngles(tempGoal))
            {

                //Debug.WriteLine("MoveToAngles:" + tempGoal.getJoint(0) + "," + tempGoal.getJoint(1) + "," + tempGoal.getJoint(2) + "," + tempGoal.getJoint(3) + "," + tempGoal.getJoint(4));
                startTime = DateTime.Now;
                //robotObject.PIDObject.desiredPos = tempGoal;
                //robotObject.PathPlanningObject.desiredPosTemp = new PathPoint(tempGoal);
                robotObject.PathPlanningObject.goal = tempGoal;
                robotObject.PathPlanningObject.RelaxGrid();
                inter1 = DateTime.Now;
                int lastPathQueueIndex = robotObject.PathPlanningObject.pathQueue.Count - 1;
                int lastPathIndex = robotObject.PathPlanningObject.pathQueue[lastPathQueueIndex].Count - 1;
                robotObject.PathPlanningObject.BuildPathSection(args.command, robotObject.PathPlanningObject.pathQueue[lastPathQueueIndex][lastPathIndex].getEndPos() );   
                //robotObject.PathPlanningObject.BuildPathSection(robotObject.PathPlanningObject.getDesiredPos(gridconst.planningDelay ));
                //robotObject.PathPlanningObject.BuildPathPoints(robotObject.PathPlanningObject.getDesiredPos(0.3));
                inter2 = DateTime.Now;
                //robotObject.PathPlanningObject.BuildPathSections();
                //inter3 = DateTime.Now;
                time1 = inter1 - startTime;
                time2 = inter2 - inter1;
                //time3 = inter3 - inter2;
                Debug.WriteLine("Time to Relax Grid:" + time1.TotalMilliseconds + " msecs");
                Debug.WriteLine("Time to Build path:" + time2.TotalMilliseconds + " msecs");
                //Debug.WriteLine("Time to Build Path Sections:" + time3.TotalMilliseconds + " msecs");

                //robotObject.PPinitialised = true;
            }
            else
            {
                //what to do if XYZ unreachable
                throw new Exception("XYZ not reachable unresolved");
            }
            //currentPos = robotObject.PIDObject.CurrentPos.get();
            //robotObject.PathPlanningObject.goal = tempGoal;
            ////robotObject.PathPlanningObject.BuildGridSearchList;
            //startTime = DateTime.Now;
            //robotObject.PathPlanningObject.RelaxGrid();
            //inter1 = DateTime.Now;
            //robotObject.PathPlanningObject.BuildPathPoints(currentPos);
            //inter2 = DateTime.Now;
            //robotObject.PathPlanningObject.BuildPathSections();
            //inter3 = DateTime.Now;
            //time1 = inter1 - startTime;
            //time2 = inter2 - inter1;
            //time3 = inter3 - inter2;
            //Debug.WriteLine("Time to relax grid: " + time1.TotalMilliseconds + "ms, time to plan points: " + time2.TotalMilliseconds + "ms, time to plan sections: " + time3.TotalMilliseconds);
        }

        private void OnSetPIDActionRequest(RobotControlEventArgs args)
        {
            //Debug.Write("SetPID: J" + args.joint);
            //Debug.Write(" HoldPWM:" + args.holdPWM);
            //Debug.Write(" Angle:" + args.Angle[args.joint]);
            //Debug.Write(" kp:" + args.kp);
            //Debug.Write(" ki:" + args.ki);
            //Debug.WriteLine(" kd:{0:N7}", args.kd);
            robotObject.PIDObject.SetGains(args);

        }

        private void OnHomeActionRequest()
        {
            if (robotObject.PIDObject.IsActive())
            {
                Console.WriteLine("PID was active");
                robotObject.PIDObject.RequestStop();
                while (robotObject.PIDObject.IsActive()) ;
                Thread.Sleep(500);
                Console.WriteLine("JointPID thread disabled for homing");
                while (robotObject.PIDObject.IsActive()) ; // double check!
                Thread.Sleep(500);
                robotObject.controllerObject.Home((byte)Axis.All);
                robotObject.PIDObject.RequestStart();
                while (!robotObject.PIDObject.IsActive()) ;
                Console.WriteLine("JointPID thread actived after homing");
            }
            else
            {
                Console.WriteLine("PID was inactive");
                robotObject.controllerObject.Home((byte)Axis.All);
            }
        }

        private void OnLookAtActionRequest(RobotControlEventArgs args)
        {
            Position tempG = new Position { XYZd = new double[USBC.TotalAxes] { args.X, args.Y, args.Z, args.gPitch, args.gRoll, args.gOffset, 0, 0 } };
            PathPoint tempGoal = new PathPoint(tempG);
            Debug.WriteLine("LookAtCoords:" + args.X + "," + args.Y + "," + args.Z + "; Roll:" + args.gRoll + " Pitch:" + args.gPitch);
            if (DynamicControl.InverseKinematics.Inv_K.solveLookAtJointAngles(tempGoal))
            {
                Debug.WriteLine("MoveToAngles:" + tempGoal.getJoint(0) + "," + tempGoal.getJoint(1) + "," + tempGoal.getJoint(2) + "," + tempGoal.getJoint(3) + "," + tempGoal.getJoint(4));
            }
            else
            {
                //what to do if XYZ not visible
            }

        }

        private void OnPointToActionRequest(RobotControlEventArgs args)
        {
            Position tempG = new Position { XYZd = new double[USBC.TotalAxes] { args.X, args.Y, args.Z, args.gPitch, args.gRoll, args.gOffset, 0, 0 } };
            PathPoint tempGoal = new PathPoint(tempG);
            Debug.WriteLine("PointToCoords:" + args.X + "," + args.Y + "," + args.Z + "; Roll:" + args.gRoll + " Pitch:" + args.gPitch);
            if (DynamicControl.InverseKinematics.Inv_K.solvePointToJointAngles(tempGoal))
            {
                Debug.WriteLine("MoveToAngles:" + tempGoal.getJoint(0) + "," + tempGoal.getJoint(1) + "," + tempGoal.getJoint(2) + "," + tempGoal.getJoint(3) + "," + tempGoal.getJoint(4));
            }
            else
            {
                //what to do if can't point to XYZ
            }
            
        }

        public void GetCurrent(object sender, EventArgs args)
        {
            PathPoint tempPos = new PathPoint();
            tempPos = robotObject.PIDObject.CurrentPos.get();
            //Console.WriteLine(tempPos.XYZ[0] + ", " + tempPos.XYZ[1] + ", " + tempPos.XYZ[2]);
            
        }

        #endregion

        #region RobotController Callback
        private void StatusEventHandler(object sender, StatusEventArgs se)
        {
            Debug.WriteLine(se.Message);
        }
        #endregion


        #region Action Queue
        /// <summary>
        /// Internal queue of robot actions.
        /// </summary>
        private Queue<Action> actionQueue = new Queue<Action>();
        private RobotControlEventArgs HelloSecondPosition =
            new RobotControlEventArgs(ControlCommands.NULL,600,0,700,-10,-180,0);
        private RobotControlEventArgs HelloFirstPosition =
            new RobotControlEventArgs(ControlCommands.NULL, 600, 0, 700, 5, -180, 0);
        


         /// <summary>
        /// Enqueues any RobotController action.
        /// </summary>
        /// <param name="item">Action delegate.</param>
        public void EnqueueAction(Action<RobotControlEventArgs> item,RobotControlEventArgs args)
        {
            actionQueue.Enqueue(() => item(args));
        }

        /// <summary>
        /// Executes the movement queue.
        /// </summary>
        public void RunActionQueue()
        {

        }

        /// <summary>
        /// Execute the action at the front of the queue and return the number of actions left
        /// </summary>
        /// <returns></returns>
        public int ActionDeQueue()
        {
            var action = actionQueue.Dequeue();
            action();
            return actionQueue.Count;
        }
        /// <summary>
        /// Triggers the ActionCompleted event.
        /// </summary>
        public void TriggerActionCompleted()
        {
            OnActionCompleted();
        }

        /// <summary>
        /// Called when a unspecified action have completed.
        /// </summary>
        protected void OnActionCompleted()
        {
            if (ActionCompleted != null)
            {
                ActionCompleted(this, new EventArgs());
            }
        }
        
        #endregion 
    }
}
